#ifndef TURRET_H
#define TURRET_H

#include "Include.h"
#include "SpeedEncoder.h"
#include "DoubleVictor.h"
#include "ThetaPIDSource.h"
#include "TargetTracker2012.h"

namespace Team675
{

class Turret
{
public:
	Turret();
	
	void ZeroPhi();
	void ThetaChangeSetpoint(float _change);
	void ThetaSetError(float _error);
	
	void UpdateAutomatic(Target::TargetPosition _target, float _phiSetpoint, float _speedSetpoint);
	void UpdateMotorValues(float _thetaMotorValue, float _phiMotorValue, float _speedMotorValue);
	void UpdatePIDValues(float _thetaSetpointChange, float _phiSetpoint, float _speedSetpoint);
	float GetConfidence(); // [0,1] possibly binary (not yet implemented)
private:
	float GetShootingRollerSpeed(float _exitSpeed);
	float GetThetaEncoderDist(float _radians);
	float GetPhiEncoderDist(float _radians);
	
	TargetTracker2012 m_tracker;
	SendablePIDController *m_thetaPID, *m_phiPID, *m_speedPID;
	Victor *m_thetaVic, *m_phiVic;
	DoubleVictor *m_speedVic;
	Encoder *m_thetaEnc, *m_phiEnc;
	SpeedEncoder *m_speedEnc;
	Gyro *m_gyro;
	DigitalInput *m_thetaLimit, *m_phiLimit;
	bool m_canZeroPhi;
	bool m_zeroingPhi;
	Timer m_zeroingTimer;
	
	float m_confidence;
	//float m_thetaMotorValue, m_phiMotorValue, m_speedMotorValue; //used only in TRACKING_MANUAL mode
	//float m_thetaSetpoint, m_phiSetpoint, m_speedSetpoint;
};

}

#endif
